/**
 * @file example_interfaces_robot_01.cpp
 * @brief 模拟机器人服务端节点实现
 * @version 0.1
 * @date 2025-06-05
 *
 * @copyright Copyright (c) 2025
 *
 */
#include "example_ros2_interfaces/msg/robot_status.hpp"
#include "example_ros2_interfaces/srv/move_robot.hpp"
#include <chrono>
#include <cmath>
#include <functional>
#include <iostream>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <thread>

/**
 * @brief 创建一个机器人类，模拟真实机器人
 *
 * 测试指令：ros2 service call /move_robot example_ros2_interfaces/srv/MoveRobot
 * "{distance: 5}"
 */
class Robot {
public:
  Robot() = default;
  ~Robot() = default;

  /**
   * @brief 移动指定的距离
   *
   * @param distance 要移动的距离，正值前进，负值后退
   * @return float 移动后的当前位置
   */
  float move_distance(float distance) {
    status_ = example_ros2_interfaces::msg::RobotStatus::STATUS_MOVEING;
    target_pose_ += distance;

    // 当目标距离和当前距离大于0.01则持续向目标移动
    while (std::fabs(target_pose_ - current_pose_) > 0.01) {
      // 每一步移动当前到目标距离的1/10
      float step = distance / std::fabs(distance) *
                   std::fabs(target_pose_ - current_pose_) * 0.1;
      current_pose_ += step;
      std::cout << "移动了：" << step << " 当前位置：" << current_pose_
                << std::endl;
      // 当前线程休眠500ms
      std::this_thread::sleep_for(std::chrono::milliseconds(500));
    }

    status_ = example_ros2_interfaces::msg::RobotStatus::STATUS_STOP;
    return current_pose_;
  }

  /**
   * @brief 获取当前位置
   *
   * @return float 当前位置
   */
  float get_current_pose() { return current_pose_; }

  /**
   * @brief 获取机器人状态
   *
   * @return int 机器人状态码
   *  1 example_ros2_interfaces::msg::RobotStatus::STATUS_MOVEING - 移动中
   *  2 example_ros2_interfaces::msg::RobotStatus::STATUS_STOP - 已停止
   */
  int get_status() { return status_; }

private:
  // 当前位置
  float current_pose_ = 0.0;
  // 目标位置
  float target_pose_ = 0.0;
  // 机器人状态
  int status_ = example_ros2_interfaces::msg::RobotStatus::STATUS_STOP;
};

/**
 * @brief 机器人服务端节点类，提供移动服务并发布状态
 */
class ExampleInterfacesRobot : public rclcpp::Node {
public:
  /**
   * @brief 构造函数
   * @param name 节点名称
   */
  ExampleInterfacesRobot(std::string name) : Node(name) {
    RCLCPP_INFO(this->get_logger(), "节点已启动：%s.", name.c_str());

    // 创建move_robot服务
    move_robot_server_ =
        this->create_service<example_ros2_interfaces::srv::MoveRobot>(
            "move_robot",
            std::bind(&ExampleInterfacesRobot::handle_move_robot, this,
                      std::placeholders::_1, std::placeholders::_2));

    // 创建状态发布者
    robot_status_publisher_ =
        this->create_publisher<example_ros2_interfaces::msg::RobotStatus>(
            "robot_status", 10);

    // 创建一个周期为500ms的定时器
    timer_ = this->create_wall_timer(
        std::chrono::milliseconds(500),
        std::bind(&ExampleInterfacesRobot::timer_callback, this));
  }

private:
  // 机器人实例
  Robot robot;
  // 定时器，用于定时发布机器人位置
  rclcpp::TimerBase::SharedPtr timer_;
  // 移动机器人服务
  rclcpp::Service<example_ros2_interfaces::srv::MoveRobot>::SharedPtr
      move_robot_server_;
  // 发布机器人位姿的发布者
  rclcpp::Publisher<example_ros2_interfaces::msg::RobotStatus>::SharedPtr
      robot_status_publisher_;

  /**
   * @brief 定时器回调函数，500ms发布一次机器人状态
   */
  void timer_callback() {
    // 创建消息
    example_ros2_interfaces::msg::RobotStatus message;
    message.status = robot.get_status();
    message.pose = robot.get_current_pose();
    RCLCPP_INFO(this->get_logger(), "Publishing: %f", robot.get_current_pose());
    // 发布消息
    robot_status_publisher_->publish(message);
  }

  /**
   * @brief 处理移动机器人服务请求的回调函数
   *
   * @param request 请求共享指针，包含移动距离
   * @param response 响应的共享指针，包含当前位置信息
   */
  void handle_move_robot(
      const std::shared_ptr<example_ros2_interfaces::srv::MoveRobot::Request>
          request,
      std::shared_ptr<example_ros2_interfaces::srv::MoveRobot::Response>
          response) {
    RCLCPP_INFO(this->get_logger(), "收到请求移动距离：%f，当前位置：%f",
                request->distance, robot.get_current_pose());
    robot.move_distance(request->distance);
    response->pose = robot.get_current_pose();
  }
};

/**
 * @brief 主函数
 * @param argc 参数数量
 * @param argv 参数数组
 * @return int 返回状态
 */
int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node =
      std::make_shared<ExampleInterfacesRobot>("example_interfaces_robot_01");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
